#include <iostream>
#include <opencv4/opencv2/core/core.hpp>
#include <opencv4/opencv2/features2d/features2d.hpp>
#include <opencv4/opencv2/highgui/highgui.hpp>
#include <chrono>

using namespace std;
using namespace cv;

int main(int argc, char **argv) 
{
    if(argc!=3)
    {
        cout<<"usage: feature_extraction img1 img2"<<endl;
        return 1;
    }
    
    Mat img_1=imread(argv[1],cv::IMREAD_COLOR);
    Mat img_2=imread(argv[2],cv::IMREAD_COLOR);
    assert(img_1.data !=nullptr && img_2.data!=nullptr);
    
    std::vector<cv::KeyPoint> keypoints_1,keypoints_2;
    Mat descriptors_1,descriptors_2;
    Ptr<FeatureDetector> detector =ORB::create();
    Ptr<DescriptorExtractor> descriptor=ORB::create();
    Ptr<DescriptorMatcher> matcher=DescriptorMatcher::create("BruteForce-Hamming");
    
    chrono::steady_clock::time_point t1=chrono::steady_clock::now(); 
    detector->detect(img_1,keypoints_1);   //orb角点检测
    detector->detect(img_2,keypoints_2);
    
    //根据角点位置计算BRIEF描述子
    descriptor->compute(img_1,keypoints_1,descriptors_1);
    descriptor->compute(img_2,keypoints_2,descriptors_2);
    chrono::steady_clock::time_point t2=chrono::steady_clock::now();
    chrono::duration<double> time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"extract ORB cost = "<<time_used.count()<<"seconds. "<<endl;
    
    Mat outimg1;
    drawKeypoints(img_1,keypoints_1,outimg1,Scalar::all(-1),DrawMatchesFlags::DEFAULT);
    imshow("ORB features",outimg1);
    
    //对两幅图像中的BRIEF描述子匹配，使用Hamming距离
    vector<DMatch> matchers;
    t1=chrono::steady_clock::now();
    matcher->match(descriptors_1,descriptors_2,matchers);
    t2=chrono::steady_clock::now();
    time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"match ORB cost = "<<time_used.count()<<"seconds. "<<endl;
    
    //筛选匹配点，计算最大与最小距离
    auto min_max=minmax_element(matchers.begin(),matchers.end(),
                                [](const DMatch &m1,const DMatch &m2) {return m1.distance<m2.distance;});
    double min_dist = min_max.first->distance;
    double max_dist = min_max.second->distance;
    
    printf("--Max dist : %f \n", max_dist);
    printf("--min_dist : %f \n", min_dist);
    
    //根据描述子之间的距离判断匹配是否有误，考虑到最小距离太小，设定最小值的下限为30
    std::vector<DMatch> good_matches;
    for(int i=0;i<descriptors_1.rows;i++)
    {
        if(matchers[i].distance<=max(2*min_dist,30.0))
        {
            good_matches.push_back(matchers[i]);
        }
    }
    
    //绘制匹配结果
    Mat img_match;
    Mat img_goodmatch;
    drawMatches(img_1,keypoints_1,img_2,keypoints_2,matchers,img_match);
    drawMatches(img_1,keypoints_1,img_2,keypoints_2,good_matches,img_goodmatch);
    imshow("all matches",img_match);
    imshow("good_matches",img_goodmatch);
    waitKey(0);
    
    return 0;
}
